
#include <mcvip_priv.h>
#include <osa_cmem.h>

#define MCVIP_DEMUX_DEBUG

//#define MCVIP_DEBUG_HANG

#define MCVIP_XY_F0_BLANK 0xAB
#define MCVIP_XY_F0_VALID 0x80
#define MCVIP_XY_F1_BLANK 0xEC
#define MCVIP_XY_F1_VALID 0xC7

#define MCVIP_XY_INVALID  0xFF

#define MCVIP_TVP5158_METADATA_LENGTH   (4*2)

#define TVP5158_BOP       (1<<6)
#define TVP5158_EOP       (1<<5)

#define TVP5158_CH_VALID  ((1<<7)|(1<<31))
#define TVP5158_VDET      (1<<12)
#define TVP5158_EOL       (1<<13)
#define TVP5158_BOL       (1<<14)

#define TVP5158_F_BIT		  (1<<30)
#define TVP5158_DUMMY		  (0x01010101)

#define MCVIP_SKIP_COUNT  (12)
#define MCVIP_START_LINE_NUM  (0)

int MCVIP_demuxInit(MCVIP_Hndl *hndl)
{
  int i, status;
  MCVIP_BufInfo *pBufInfo;
    
  for(i=0; i<hndl->chList.numCh; i++)
  {
    hndl->chDemuxInfo[i].errorFrame=0;
    hndl->chDemuxInfo[i].errorLine=0;    
    hndl->chDemuxInfo[i].curField = 0;
    hndl->chDemuxInfo[i].curLine[0] = 0;
    hndl->chDemuxInfo[i].curLine[1] = 0;  
    hndl->chDemuxInfo[i].isFirstFrame = 1;
    hndl->chDemuxInfo[i].skipCount = MCVIP_SKIP_COUNT;
    
    hndl->chDemuxInfo[i].prevXY  = MCVIP_XY_INVALID;
    hndl->chDemuxInfo[i].prevLineNum  = MCVIP_START_LINE_NUM-1;
    
    status = OSA_queGet(&hndl->emptyQue[i], &hndl->chDemuxInfo[i].curBufId, OSA_TIMEOUT_FOREVER);
    OSA_assertSuccess(status);

    pBufInfo = MCVIP_getBufInfo((void*)hndl, hndl->chDemuxInfo[i].curBufId);
    pBufInfo->chId = i;
          
    OSA_assert(pBufInfo!=NULL);
          
    hndl->chDemuxInfo[i].curDstPhysAddr = pBufInfo->physAddr;  
    hndl->chDemuxInfo[i].curDstVirtAddr = pBufInfo->virtAddr;    
  }
  
  for(i=0; i<MCVIP_BUF_MAX; i++)
    hndl->tmpDemuxQue[i] = -1;  

  hndl->dmaDemux.srcChromaOffset = 0; // since in DM355 UV data is interleaved with Y
  hndl->dmaDemux.dstChromaOffset = 0; // since in DM355 UV data is interleaved with Y
  hndl->dmaDemux.copyWidth    = hndl->chList.info[0].width * 2; // since in DM355 UV data is interleaved with Y
  hndl->dmaDemux.srcPhysAddrList = hndl->dmaDemuxSrcPhysAddr;
  hndl->dmaDemux.dstPhysAddrList = hndl->dmaDemuxDstPhysAddr;
  
  hndl->chBase = 0;
  if( (hndl->createPrm.videoDecoderMode == MCVIP_VIDEO_DECODER_MODE_2CH_D1) ||
      (hndl->createPrm.videoDecoderMode == MCVIP_VIDEO_DECODER_MODE_2CH_D1_CROP) ) 
  {
    hndl->chBase = 2;    
  }
   
  hndl->initTimeInMsecs = OSA_getCurTimeInMsec();
  
  return OSA_SOK;
}

int MCVIP_demuxRun(MCVIP_Hndl *hndl, MCVIP_V4l2Buf *buf)
{
  int tmpDemuxQueIdx=0, i, numCh, eol, extraCh;
  volatile Uint8 *curAddr;
  Uint16 chId, lineNum=0;
  Uint8 *destAddr;
  Uint8 *srcAddr;
  Bool isDone;
  MCVIP_ChDemuxInfo *pChInfo;
  MCVIP_BufInfo *pBufInfo;
  Uint32 destAddrOffset, srcAddrOffset, dstLineOffsetH, curTime;
  volatile Uint32 metaData, validFlag;
  Uint8 curXY;
  #ifdef MCVIP_DEMUX_DEBUG
  int dummy_cnt=0, valid_cnt=0, ch_cnt[10]={0,0,0,0,0,0,0,0,0,0};
  #endif
  int videoDecoderMode;
    
  hndl->dmaDemux.numLines     = 0;
  
  dstLineOffsetH = hndl->chList.info[0].offsetH;
  numCh = hndl->chList.numCh;

  curAddr = buf->virtAddr;
  
  validFlag = TVP5158_CH_VALID|TVP5158_VDET|TVP5158_EOL|TVP5158_BOL;
  
  videoDecoderMode = hndl->createPrm.videoDecoderMode;

  curTime = (OSA_getCurTimeInMsec() - hndl->initTimeInMsecs)*1000; // in usecs
    
  for(i=0; i<numCh; i++) 
  {
    pChInfo = &hndl->chDemuxInfo[i];
    
    if(pChInfo->isFirstFrame) 
    {          
      if(pChInfo->skipCount)
      {
        pChInfo->skipCount--;
      }
    } 
    else 
    {
      pChInfo->skipCount = 0;
    }
  }
  
//  OSA_cmemCacheInv(buf->virtAddr, hndl->v4l2FrameInfo.offsetH*2*hndl->v4l2FrameInfo.height);

  for(i=0; i<hndl->v4l2FrameInfo.height; i++) 
  {  
    //metaData = *(volatile Uint32*)curAddr;
        
    metaData =  ((Uint32)curAddr[1] << 0 ) |
                ((Uint32)curAddr[3] << 8 ) |
                ((Uint32)curAddr[5] << 16 ) |
                ((Uint32)curAddr[7] << 24 );                
                
    //curAddr[1]=curAddr[3]=curAddr[5]=curAddr[7]=0;
                    
    eol     = 0;
    extraCh = 0;
    chId    = 0xFF;

    if( (metaData & TVP5158_CH_VALID) == TVP5158_CH_VALID )        
    {
      if((metaData & TVP5158_VDET)==0) {
        chId = metaData & 0x07; 
        pChInfo = &hndl->chDemuxInfo[chId];
        pChInfo->curLine[0]=0; 
        pChInfo->curLine[1]=0;        
        pChInfo->isFirstFrame = 1;   
        pChInfo->errorFrame = 0;
        pChInfo->errorLine = 0;        
        pChInfo->prevLineNum = MCVIP_START_LINE_NUM-1;
        pChInfo->skipCount = MCVIP_SKIP_COUNT;
        chId = 0xFF;
      }
    }

    if( (metaData & validFlag) == validFlag )
    {
      chId = metaData & 0x07;
      
      #ifdef MCVIP_DEMUX_DEBUG      
      valid_cnt++;
      ch_cnt[chId]++;
      #endif
      
      if(hndl->chBase) {
        if(chId>=hndl->chBase)
          chId = chId - hndl->chBase;
      }
              
    } 
    else if( (metaData & (TVP5158_CH_VALID|TVP5158_VDET)) == (TVP5158_CH_VALID|TVP5158_VDET)) 
         {
           // extra D1 channel
           chId = numCh-1;
           extraCh = 1;      
           if(metaData & TVP5158_EOL)
           {
             eol = 1;
           }

           #ifdef MCVIP_DEMUX_DEBUG      
           valid_cnt++;
           ch_cnt[chId+eol]++;
           #endif
         }
         #ifdef MCVIP_DEMUX_DEBUG
         else 
         {
           if (metaData == TVP5158_DUMMY)
           {
             dummy_cnt++;
           }
           else 
           {
//               OSA_printf("i=%4d f=%4d %X\n",i,hndl->frameCount, metaData);
           }

           if (i== hndl->v4l2FrameInfo.height-1) 
           {
             #if 1
//             OSA_printf("%d %d %d %d ", ch_cnt[0],ch_cnt[1],ch_cnt[2],ch_cnt[3]);
//             OSA_printf("%d %d %d %d ", ch_cnt[4],ch_cnt[5],ch_cnt[6],ch_cnt[7]);
//             OSA_printf("%d ", ch_cnt[0]+ch_cnt[1]+ch_cnt[2]+ch_cnt[3]+ch_cnt[4]+ch_cnt[5]);
//             OSA_printf("dummy %d v=%d %d\n", dummy_cnt,valid_cnt,valid_cnt+dummy_cnt);
             #endif
           }
         }    
         #endif
      
    if(chId < numCh) 
    {    
      lineNum = ((metaData & 0x007F0000) >> 16)|((metaData & 0x00000300) >> 1);
      

      
      if(lineNum>(hndl->chList.info[chId].height/2 - 1)) 
      {
        chId = 0xFF; //skip line
      }
    }    
    
    #ifdef MCVIP_DEMUX_DEBUG
    if (i==0) 
    {//      OSA_printf("%d %d\n", hndl->frameCount, buf->id);	    
    }
    
    if (i==16) 
    {      //OSA_printf("\n");
    }

    //if ((chId==0) | (metaData == TVP5158_DUMMY))
    {
      if (i<16 ) 
      {
        //OSA_printf("c%1d %4d %4d\n", chId, lineNum,i);
      }
      else if (i>hndl->v4l2FrameInfo.height-4) 
           {
             //OSA_printf("c%3d %4d %4d %0X\n", chId, lineNum,i,metaData);
           }
    }
    #endif
        
    if(chId < numCh) 
    {    
      pChInfo = &hndl->chDemuxInfo[chId];  

      curXY = metaData >> 24;
      
      if(curXY == MCVIP_XY_F0_VALID || curXY == MCVIP_XY_F0_BLANK) 
      { 
        pChInfo->curField = 0;
      } 
      else if(curXY == MCVIP_XY_F1_VALID || curXY == MCVIP_XY_F1_BLANK) 
           {
             pChInfo->curField = 1;
           } 
           else
           {
             chId = 0xFF;
           }
      
      #if 1 
      if(videoDecoderMode == MCVIP_VIDEO_DECODER_MODE_4CH_CIF              ||
         videoDecoderMode == MCVIP_VIDEO_DECODER_MODE_4CH_CIF_CROP         ||   
         videoDecoderMode == MCVIP_VIDEO_DECODER_MODE_8CH_CIF              ||
         videoDecoderMode == MCVIP_VIDEO_DECODER_MODE_8CH_CIF_CROP         ||
         videoDecoderMode == MCVIP_VIDEO_DECODER_MODE_4CH_CIF_PLUS_D1      ||
         videoDecoderMode == MCVIP_VIDEO_DECODER_MODE_4CH_CIF_PLUS_D1_CROP ||
         videoDecoderMode == MCVIP_VIDEO_DECODER_MODE_8CH_CIF_PLUS_D1      ||
         videoDecoderMode == MCVIP_VIDEO_DECODER_MODE_8CH_CIF_PLUS_D1_CROP ) 
      {        
        if((extraCh != 1) || (chId != (numCh - 1))) 
        { 
          pChInfo->curField ^= 1;
        } 
      }
      #endif
    }

    if(chId < numCh) 
    {    
      if(pChInfo->skipCount)
      { 
        chId = 0xFF;
      }
        
      if(chId < numCh) 
      {            
          
        if(pChInfo->isFirstFrame && pChInfo->curField == 1 ) 
        {
          // sync to even field
          chId = 0xFF;
          //OSA_printf(" MCVIP: Syncing to even field\n");

        } 
        else 
        {
          pChInfo->isFirstFrame = 0;
        }
      }
    }

    if(chId < numCh) 
    {        
      pChInfo->curLine[pChInfo->curField] = lineNum;
      
      if(pChInfo->prevLineNum>=0) 
      {
        if( (pChInfo->prevLineNum+1) != lineNum) 
        {
          pChInfo->errorFrame = 1;
          pChInfo->errorLine++;
          OSA_printf("ch=%d L=(%d, %d) i=%d\n", chId,lineNum, pChInfo->prevLineNum, i);
          
          #ifdef MCVIP_DEMUX_DEBUG
      	  if (chId == 0) 
	  {
            //OSA_printf("err: %4d %4d\n", lineNum,i);
	  }
	  #endif
        }
      } 
      else 
      {
        if( lineNum > 1 ) 
        {
          pChInfo->errorFrame = 1;
          pChInfo->errorLine++;
        }
      }
      
      if( (!extraCh) || (extraCh && eol) ) 
      {        
        pChInfo->prevLineNum  = lineNum;        
      }
     
      destAddrOffset =  2 * pChInfo->curLine[pChInfo->curField] * dstLineOffsetH
                        +  pChInfo->curField * dstLineOffsetH
                        + eol*hndl->chList.info[0].width;
                          
      destAddrOffset *= 2;
                                
      srcAddrOffset = (curAddr-buf->virtAddr)+MCVIP_TVP5158_METADATA_LENGTH;

      destAddr = pChInfo->curDstPhysAddr + destAddrOffset;
      srcAddr  = buf->physAddr + srcAddrOffset;
        
      hndl->dmaDemuxSrcPhysAddr[hndl->dmaDemux.numLines] = (unsigned long)srcAddr;
      hndl->dmaDemuxDstPhysAddr[hndl->dmaDemux.numLines] = (unsigned long)destAddr;        
      hndl->dmaDemux.numLines++;

      isDone = FALSE;     
      
      if(lineNum==(hndl->chList.info[chId].height/2 - 1))  
      {
        pChInfo->prevLineNum = MCVIP_START_LINE_NUM-1;
      }

      if(pChInfo->curLine[0]== (hndl->chList.info[chId].height/2 - 1) && 
         pChInfo->curLine[1]== (hndl->chList.info[chId].height/2 - 1)) 
      {        
        // valid frame, all lines received
        if(extraCh) 
        {
          if(eol)
          {
            isDone = TRUE;
          }
            
        } 
        else 
        {
          isDone = TRUE;            
        }
      } 

      if(isDone) 
      {
        pChInfo->curLine[0] = 0;
        pChInfo->curLine[1] = 0;     
        
        pChInfo->isFirstFrame = 1;
        pChInfo->skipCount = 0; 
        pChInfo->prevLineNum = MCVIP_START_LINE_NUM-1;              
      
        if(!pChInfo->errorFrame || (pChInfo->errorFrame && pChInfo->errorLine < 3)) 
        {        
          hndl->tmpDemuxQue[tmpDemuxQueIdx] = pChInfo->curBufId;
          tmpDemuxQueIdx++;
          
          pBufInfo = MCVIP_getBufInfo((void*)hndl, pChInfo->curBufId);
          pBufInfo->timestamp = curTime;

          #ifdef MCVIP_DEBUG_HANG
          OSA_printf(" Demux   Ch %d: Buf %d\n", chId, pChInfo->curBufId);
          #endif
          
          OSA_prfBegin(&hndl->prfQueWait);
          
          OSA_queGet( &hndl->emptyQue[chId], &pChInfo->curBufId, OSA_TIMEOUT_FOREVER);
                    
          OSA_prfEnd(&hndl->prfQueWait, 1);
          
          if(pChInfo->curBufId == 0xFF)
          {
            return OSA_EFAIL;
          } 
            
          #ifdef MCVIP_DEBUG_HANG                  
          OSA_printf(" Demux   Ch %d: New %d\n", chId, pChInfo->curBufId);
          #endif
          
          pBufInfo = MCVIP_getBufInfo((void*)hndl, pChInfo->curBufId);
                    
          OSA_assert(pBufInfo!=NULL);
          
          pBufInfo->chId = chId;          
          
          pChInfo->curDstVirtAddr = pBufInfo->virtAddr;  
          pChInfo->curDstPhysAddr = pBufInfo->physAddr;                    
        }
        
        if(pChInfo->errorFrame) 
        {
//          OSA_printf(" Frame %5d: Missing lines %d ch=%d i=%d\n", hndl->frameCount, pChInfo->errorLine,chId,i);
        }

        pChInfo->errorFrame = 0;
        pChInfo->errorLine = 0;        
      }        
    } 
    
    curAddr += hndl->v4l2FrameInfo.offsetH * 2;
  } 


  OSA_prfBegin(&hndl->prfDma);

  DRV_dmaDemux(&hndl->dmaDemuxHndl, &hndl->dmaDemux); 
    
  OSA_prfEnd(&hndl->prfDma, 1);  

  #ifdef MCVIP_DEBUG_HANG
  OSA_printf(" Demux End\n");
  #endif

  for(i=0; i<tmpDemuxQueIdx; i++) 
  {
    if(hndl->tmpDemuxQue[i]>=0) 
    {
//      OSA_printf(" Demux Put   : Buf %d\n", hndl->tmpDemuxQue[i]);
      OSA_quePut(&hndl->fullQue, hndl->tmpDemuxQue[i], OSA_TIMEOUT_FOREVER);
      hndl->tmpDemuxQue[i] = -1;
    }
  }

  return OSA_SOK;
}

